//
// Created by HuYunhao on 2020/12/25.
//

#include "stanley_controller.h"

namespace controlNS {
//读取配置文件中的控制参数
fstream json_file("/home/hyh/coder/cpp/ros_simulink/src/config/controlParam.json");
nlohmann::json control_json = nlohmann::json::parse(json_file);
const float kStanleyGain = control_json.at("kStanleyGain");
const float kSpeedPGain = control_json.at("kSpeedPGain");
const float kTimeDifference = control_json.at("kTimeDifference");
const float kWheelBase = control_json.at("kWheelBase");
const float kMaxSteerAngle = control_json.at("kMaxSteerAngle");
const float kSteeringRatio = control_json.at("kSteeringRatio");
float PidControl(float target_velocity, float current_velocity) {
    return kSpeedPGain * (target_velocity - current_velocity);
}


void State::Update(float acceleration, float delta) {
    //限定方向盘转角范围，使得delta∈[-kMaxSteerAngle,kMaxSteerAngle]
    delta = min(kMaxSteerAngle, max(delta, -kMaxSteerAngle));
    //车辆的自行车模型（bicycle model）更新方程
    this->x_ += this->v_ * cos(this->yaw_) * kTimeDifference;
    this->y_ += this->v_ * sin(this->yaw_) * kTimeDifference;
    this->yaw_ += this->v_ * tan(delta) / kWheelBase * kTimeDifference;
    //限制横摆角的范围，yaw∈[-pi,pi]
    this->yaw_ = myNumpy::normalizeAngle(this->yaw_);
    this->v_ += acceleration * kTimeDifference;

}

void State::UpdateForPreScan(float x, float y, float yaw, float v) {
    this->x_ = x;
    this->y_ = y;
    this->yaw_ = myNumpy::normalizeAngle(yaw);
    this->v_ = v;

}

State::State(float x, float y, float yaw, float v) {
    this->x_ = x;
    this->y_ = y;
    this->yaw_ = myNumpy::normalizeAngle(yaw);
    this->v_ = v;
}


StanleyController::StanleyController() {
    this->delta_ = 0.0f;    //初始化车轮转角[rad]
}

StanleyController::~StanleyController() {

}

void StanleyController::calcNearestIndex() {
    float dx, dy, ds, min_distance = 999.99f;
    for (int i = 0; i < cx_.size(); i++) {
        dx = this->fx_ - cx_[i];
        dy = this->fy_ - cy_[i];
        ds = pow(dx, 2) + pow(dy, 2);
        if (i == 0 || ds < min_distance) {
            min_distance = ds;
            this->min_distance_index_ = i;
        }
    }
}

float StanleyController::calcDelta(const State &state) {
    // 1 规范车速
    float vehicle_speed = state.v_ > 1 ? state.v_ : 1.0f;
    this->calcPath(state);      //得出车辆前轮中心的坐标[fx_, fy_, yaw_],读取规划路径
    this->calcNearestIndex();   //计算参考点的索引
    this->calcError();          //计算横向偏差和航向偏差
    // 2 stanley算法
    // 2.1 计算stanley算法中的theta_e
    float theta_e = this->heading_error_;
    // 2.2 计算stanley算法中的theta_d
    float theta_d = atan2f(kStanleyGain * this->cross_track_error_, vehicle_speed);
    // 2.3 得出转角
    float delta = theta_e + theta_d;    //车轮转角[rad]
    float limit_steer_angle = myNumpy::deg2rad(kMaxSteerAngle);
    delta = min(limit_steer_angle, max(delta, -limit_steer_angle));   //限制车轮转角的范围为[-kMaxSteerAngle, kMaxSteerAngle]
    delta = myNumpy::lowerPass(delta, this->delta_);    //将车轮转角进行一阶低通滤波
    this->delta_ = delta;               //储存本次的计算值
    return delta;
}

void StanleyController::calcPath(const State &state) {
    this->fx_ = state.x_ + kWheelBase * cos(state.yaw_);
    this->fy_ = state.y_ + kWheelBase * sin(state.yaw_);
    this->yaw_ = state.yaw_;
    this->cx_ = myNumpy::readTxt<float>(control_json.at("path_x"));    //读取路径点信息,并且储存到类Stanley中
    this->cy_ = myNumpy::readTxt<float>(control_json.at("path_y"));
    this->cyaw_ = myNumpy::readTxt<float>(control_json.at("path_yaw"));
}

void StanleyController::calcError() {
    int i = this->min_distance_index_;
    float dx = this->fx_ - this->cx_[i];
    float dy = this->fy_ - this->cy_[i];
    this->cross_track_error_ = dx * sin(this->cyaw_[i]) - dy * cos(this->cyaw_[i]);
    this->heading_error_ = myNumpy::normalizeAngle(this->cyaw_[i] - this->yaw_);  //路径切线角-车辆的yaw angle

}


}; //   namespace controlNS
